#include "openmv.h"

int16_t gimbal_yaw = 180;
int16_t gimbal_pitch = 90;

void openmv_receive_data(uint8_t *data, uint16_t size)
{
		
	//解析数据

	if(data[0] == 0xA5 && data[size - 1] == 0xA6)
	{
		if( size == 6)//读取数据长度
		{
			gimbal_yaw = (data[1] << 8 | data[2]);
			gimbal_pitch = (data[3] << 8 | data[4]);
			
			Servo_SetYaw(gimbal_yaw);
            Servo_SetPitch(gimbal_pitch);
//			printf("OpenMV x: %d ,y :%d\n", x,y);
			
		}
	}
	else 
	{
//		printf("无效!\n");
	}
			
}

void openmv_task(void)
{
    if(my_uart6.data_flag)
    {
        openmv_receive_data(my_uart6.ReceiveData, my_uart6.ReceveNum);
        my_uart6.data_flag = 0;  // 清除标志
    }
}
